Let me know if you want to discuss something online? I'll not be in the lab over spring break.

We had discussed the possibility of taking c3 objects directly, Was this not fitting this construct well?

@Meow404 reviewed 4 files and all commit messages, and made 13 comments.
Reviewable status: 4 of 9 files reviewed, 12 unresolved discussions (waiting on ebianchi).

core/c3.h line 59 at r1 (raw file):

    std::vector<Eigen::MatrixXd> G;
    std::vector<Eigen::MatrixXd> U;
    std::vector<Eigen::MatrixXd> Q_evaluation;

Do you still need this?

core/c3.cc line 431 at r1 (raw file):

  }
  vector<VectorXd> delta(N_, delta_init);
  vector<VectorXd> w(N_, VectorXd::Zero(n_z_));

Thank you for taking care of the namespaces.

core/traj_eval.h line 47 at r3 (raw file):

    int n_data = data[0].size();

any reason why the logic is kept in the header file?

core/traj_eval.h line 68 at r3 (raw file):

      const std::vector<Eigen::VectorXd>& data,
      const std::vector<Eigen::VectorXd>& data_des,
      const Eigen::MatrixXd& cost_matrix, Rest&&... rest) {

Isn't there a verson of this where the data_des is a single Eigen::VectorXd instead of an std::vector<Eigen::VectorXd>? Like x_des in c3.

core/traj_eval.h line 73 at r3 (raw file):

        std::forward<Rest>(rest)...);
  }
  /** Special case: no trajectory to evaluate. */

This can be a private/protected function

core/traj_eval.h line 81 at r3 (raw file):

   * feedforward control), and return the resulting trajectory of actual states
   * and inputs.
   *

Im wondering if a method to generate a fine_lcs from a coarse_lcs may be helpful. From the push_anything_dev maybe the ability to up-sample the LCS? We could leave G and U empty and potentially allow users to just send a single lcs. , It also would allow them to reuse the function if needed.

core/traj_eval.h line 104 at r3 (raw file):

                           const std::vector<Eigen::VectorXd>& u_plan,
                           const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd,
                           const LCS& coarse_lcs, const LCS& fine_lcs);

Why do we need the coarse_lcs? From the logic used in the file it seems the essential variable is timestep_split and fine_lcs. I think it's a bit contextual, but what about having a function that taken in timestep_split and is called by this function? (that way if I know my timestep_split, i dont have to regenerate an LCS)

core/traj_eval.cc line 75 at r3 (raw file):

  // Zero-order hold the planned trajectory to match the finer time
  // discretization of the LCS.
  std::pair<vector<VectorXd>, vector<VectorXd>> fine_plans =

does auto [x_plan_finer, u_plan_finer] = ZeroOrderHoldTrajectories(... work here? same for the others.

core/traj_eval.cc line 97 at r3 (raw file):

vector<VectorXd> TrajectoryEvaluator::SimulateLCSOverTrajectory(
    const VectorXd& x_init, const vector<VectorXd>& u_plan, const LCS& lcs) {

i think you should pass a variable of type LCSSimulateConfig, refer to lcs.h

core/traj_eval.cc line 114 at r3 (raw file):

    const vector<VectorXd>& x_plan, const vector<VectorXd>& u_plan,
    const LCS& lcs) {
  return SimulateLCSOverTrajectory(x_plan[0], u_plan, lcs);

Im looking at

  // Use the C3 plan.
  else if (cost_type == C3CostComputationType::kUseC3Plan) {
    for (int i = 0; i < N_ * resolution; i++) {
      UU[i] = z_fin[i / resolution].segment(n_x_ + n_lambda_, n_u_);
      XX[i] = z_fin[i / resolution].segment(0, n_x_);
      if (i == N_ - 1) {
        XX[i + 1] = lcs_for_cost.Simulate(XX[i], UU[i], simulate_config);
      }
    }
  }

from the push_anything_dev branch, what about simulating one step for every point of the x trajectory to create a new trajectory?

core/traj_eval.cc line 164 at r3 (raw file):

vector<VectorXd> TrajectoryEvaluator::DownsampleTrajectory(
    const vector<VectorXd>& fine_traj, const int& timestep_split) {

is upsample_rate more verbose instead of timestep_split?

core/traj_eval.cc line 198 at r3 (raw file):

  int timestep_split = fine_lcs.N() / coarse_lcs.N();
  DRAKE_THROW_UNLESS(fine_lcs.dt() * timestep_split == coarse_lcs.dt());
  DRAKE_THROW_UNLESS(coarse_lcs.N() * timestep_split == fine_lcs.N());

are the last two checks redundant? or is it to make sure fine_lcs.N() is divisible by coarse_lcs.N()

Thanks for the reminder. I've added a C3 object as a possible single input argument to ComputeQuadraticTrajectoryCost as well as a test for it. I put that test in C3CartpoleTypedTest instead of TrajectoryEvaluatorTest so that I could reuse the cartpole's C3 object, even though its purpose is to test this aspect of the trajectory evaluation. Lmk if you think this is a problem.

@ebianchi made 13 comments.
Reviewable status: 4 of 9 files reviewed, 12 unresolved discussions (waiting on Meow404).

core/c3.h line 59 at r1 (raw file):

Good catch, thanks! Done.

core/c3.cc line 431 at r1 (raw file):

Done.

core/traj_eval.h line 47 at r3 (raw file):

It seems to be a quirk of variadic templates. If the function implementation is in the .cc file, then the .cc file also needs explicit instantiations of every argument pattern that could be used. This would prevent calling ComputeQuadraticTrajectoryCost with an arbitrary number of inputs. Keeping the function implementation in the .h file instead lets us avoid explicit instantiation, and the variadic templating works as we initially expected (with maximal flexibility and minimal verbosity).

This seems like a bit of a weird quirk to me. It still seems like the variadic templating is the cleanest way to go, but lmk if you think otherwise given this.

core/traj_eval.h line 68 at r3 (raw file):

Done.

core/traj_eval.h line 73 at r3 (raw file):

Done.

core/traj_eval.h line 81 at r3 (raw file):

I considered this, and I concluded I don't believe it is possible generically to generate a fine_lcsfrom a coarse_lcs. The dt that defines the coarse_lcs is incorporated into portions of the LCS matrices, in a way that I believe is only discernible when the LCS is created from a multibody plant (i.e. it is not detectible from the LCS matrices themselves afterwards). Thus creating a fine_lcs that involves changing the dt embedded in the LCS matrices is infeasible. Since doing so requires the Drake MultibodyPlant, LCSFactory already offers this functionality (coarse_lcs is defined by a call to LCSFactory with a plant,LCSFactoryOptions.dt something long, and LCSFactoryOptions.N a small number, and fine_lcs is defined by the same LCSFactory call but with shorter dt and correspondingly largerN specified).

core/traj_eval.h line 104 at r3 (raw file):

You're right in that coarse_lcs is not strictly needed to implement this function. However, I used it for performing checks ensuring the two LCSs are compatible with each other, which would prevent a user from attempting to apply a plan generated w.r.t. some LCS to a different one that isn't compatible (they must share the same time horizon and have different dt and N that are equal integer multiples of each other). The timestep_split is detectible from two provided compatible LCSs, so it was not made an additional input argument.

We could instead write this as SimulatePDControlwithLCS(... fine_lcs, timestep_split)(dropping coarse_lcs), but this prevents the ability to run LCS compatibility checks.

core/traj_eval.cc line 75 at r3 (raw file):

Done.

core/traj_eval.cc line 97 at r3 (raw file):

Done.

core/traj_eval.cc line 114 at r3 (raw file):

I excluded in this PR any functionality for cost computation that we didn't end up using in any sampling C3 examples. This includes kUseC3Plan as well as your one-step simulation for multi-timesteps suggestion. Do you think we should include cost computation strategies that we did not determine were experimentally useful?

core/traj_eval.cc line 164 at r3 (raw file):

I like it. Renamed.

core/traj_eval.cc line 198 at r3 (raw file):

Among the last 3 checks, any 1 of them can be considered redundant and removed. I removed the last one for conciseness.

Minor comments! Thanks for the work.

@Meow404 made 4 comments and resolved 10 discussions.
Reviewable status: 4 of 9 files reviewed, 3 unresolved discussions (waiting on ebianchi).

core/traj_eval.h line 47 at r3 (raw file):

That's interesting to know. This is fine then, would you be able to add a comment here stating this for future reference.

core/traj_eval.cc line 114 at r3 (raw file):

I'm not sure about it tbh. We can be add it in later if its needed.

core/traj_eval.h line 102 at r4 (raw file):

  template <typename... Rest>
  static double ComputeQuadraticTrajectoryCost(
      const std::vector<Eigen::VectorXd>& data,

I think this function maybe causing the tests to fail in CI (although its NOT failing on my own PC). Any strong motivation to keep them?

Thanks for the round of comments! I made minor changes:

1. I addressed each comment.
2. I also added in a couple new input argument configurations (and tests) for ComputeQuadraticTrajectoryCost so that a C3 object and different cost matrices from those contained within the C3 object can determine the cost. I realized this is functionality we'd need to implement one of the cost calculation types used by Push Anything and other sampling C3 examples.
3. I merged main into this branch to confirm there will be no merge conflicts when this PR is done.

@ebianchi made 4 comments.
Reviewable status: 4 of 9 files reviewed, 3 unresolved discussions (waiting on Meow404).

core/traj_eval.h line 47 at r3 (raw file):

Done.

core/traj_eval.h line 102 at r4 (raw file):

That's strange. Yes there's motivation to keep this: this version is called from the C3 object-based one: See ComputeQuadraticTrajectoryCost(c3, Q, R) which calls ComputeQuadraticTrajectoryCost(x, x_des, Q, u, R).

core/traj_eval.cc line 114 at r3 (raw file):

Ok, sgtm.